/*
 * TestAvoidObstacles.java
 *
 * Created on 12 de noviembre de 2007, 07:16 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;

import Behaviors.Arbitrators.CooperativeArbitrator;
import Behaviors.Schemas.Motor.NxMSAvoidObstacles;
import Behaviors.Schemas.Motor.NxMSGoToPoint;
import Behaviors.Schemas.Motor.NxMSNoise;
import Behaviors.Schemas.Perception.*;
import Behaviors.Schemas.Behavior;
import Robot.*;
import Robot.Sensors.*;
import Utils.*;
import java.util.Vector;
/**
 *
 * @author alejo
 */
public class TestAvoidObstacles {
    
    /**
     * Creates a new instance of TestAvoidObstacles
     */
 public static void process() {
        

     /*
       
    
    //***Definicion Robot 1*************************************
    //Robot robotFollower=new Robot("127.1.1.1", 6665, null);
    Robot marlin=new Robot("localhost",6665,"Marlin",0);
    
    //Robot camara=new Robot("127.0.0.1",6668,"Camara",0);
    /*
    Position2d positionFollower;
    try{
        positionFollower=new Position2d(robotFollower,"position1");
    }
    catch(Exception e){
        System.out.println("Error connecting to Player!");
        System.exit(1);
        return;
    }
    
    //***Definicion Robot 3*************************************
    
    RealPosition2d posReal;

    try{

        posReal = new RealPosition2d(real,"realPos",0);

    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
   
    
    marlin.runThreaded(10,0);
   
    
    Vector elements = new Vector(1,1);
    
        PSDetectObstaclesSonar detect = new PSDetectObstaclesSonar("ObstBlob",positionMarlin,10);
        
        NxMSGoToPoint goto2=new NxMSGoToPoint("goto2",positionMarlin,point,NxMSGoToPoint.MODE_EXPONENTIAL);
        NxMSAvoidObstacles avoid = new NxMSAvoidObstacles("avoid",detect,positionMarlin, NxMSAvoidObstacles.MODE_EXPONENTIAL);
        avoid.setExponentialDistance(50);
        avoid.setMaxRange(30);
        NxMSNoise noise = new NxMSNoise("noise");
        noise.setGain(0.0f);
        
        Behavior[] schemaArray = {(Behavior)goto2,(Behavior)avoid,(Behavior)noise};
        //System.out.println(schemaArray.length);
        //DetectFormation detectFormation=new DetectFormation(position1, positionCenter);
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        CooperativeArbitrator arbitrator1=new CooperativeArbitrator(schemaArray,positionMarlin,false);
        
        BehaviorBasedController controller=new BehaviorBasedController("avoidObst",marlin,arbitrator1,10);
       
        
        Thread hilo=new Thread(controller);
        
        //arbitrator1.setDebugMode(true);
        hilo.start();
        
        
            
        /*positionLeader.setSpeed(0,-1);
        while(true)
        {
            
            try{
                Thread.sleep(100);
            }
            catch(Exception e){}
            
        }*/
    }
}

